#include "stdafx.h"
#include "gp_XYZ.h"
#include "gp_Mat.h"
#include "gp.h"


 gp_XYZ::gp_XYZ () : x(0.), y(0.), z(0.) { }

 gp_XYZ::gp_XYZ (const double X,
		       const double Y,
		       const double Z) : x(X),y(Y),z(Z) { }

 void gp_XYZ::SetCoord (const double X,
			      const double Y,
			      const double Z)
{ x = X;  y = Y;  z = Z; }

 void gp_XYZ::SetCoord (const int i,
			      const double X) {
Standard_OutOfRange_Raise_if(i < 1 || i > 3, NULL);

  (&x)[i-1] = X;
}

 void gp_XYZ::SetX (const double X)
{ x = X; }

 void gp_XYZ::SetY (const double Y)
{ y = Y; }

 void gp_XYZ::SetZ (const double Z) 
{ z = Z; }

 double gp_XYZ::Coord (const int i) const {
Standard_OutOfRange_Raise_if(i < 1 || i > 3, NULL);
  return (&x)[i-1];
}

 void gp_XYZ::Coord (double& X,
			   double& Y,
			   double& Z) const
{ X = x; Y = y; Z = z; }

 double gp_XYZ::X () const
{ return x; }

 double gp_XYZ::Y () const
{ return y; }

 double gp_XYZ::Z () const
{ return z; }

 double gp_XYZ::Modulus () const { 
  return sqrt (x * x + y * y + z * z);
}

 double gp_XYZ::SquareModulus () const {
  return (x * x + y * y + z * z);
}

 void gp_XYZ::Add (const gp_XYZ& Other)
{
  x += Other.x;
  y += Other.y;
  z += Other.z;
}

 gp_XYZ gp_XYZ::Added (const gp_XYZ& Other) const {
  return gp_XYZ(x + Other.x,y + Other.y,z + Other.z);
}

 void gp_XYZ::Cross (const gp_XYZ& Right)
{
  double Xresult = y * Right.z - z * Right.y;
  double Yresult = z * Right.x - x * Right.z;
  z                     = x * Right.y - y * Right.x;
  x = Xresult;
  y = Yresult;
}

 gp_XYZ gp_XYZ::Crossed (const gp_XYZ& Right) const
{
  return gp_XYZ (y * Right.z - z * Right.y,
		 z * Right.x - x * Right.z,
		 x * Right.y - y * Right.x);
}

 double gp_XYZ::CrossMagnitude (const gp_XYZ& Right) const
{
  double Xresult = y * Right.z - z * Right.y;
  double Yresult = z * Right.x - x * Right.z;
  double Zresult = x * Right.y - y * Right.x;
  return sqrt(Xresult * Xresult + Yresult * Yresult + Zresult * Zresult);
}

 double gp_XYZ::CrossSquareMagnitude (const gp_XYZ& Right) const
{
  double Xresult = y * Right.z - z * Right.y;
  double Yresult = z * Right.x - x * Right.z;
  double Zresult = x * Right.y - y * Right.x;
  return Xresult * Xresult + Yresult * Yresult + Zresult * Zresult;
}

 void gp_XYZ::CrossCross (const gp_XYZ& Coord1,
				const gp_XYZ& Coord2)
{
  double Xresult = 
    y * (Coord1.x * Coord2.y - Coord1.y * Coord2.x) -
      z * (Coord1.z * Coord2.x - Coord1.x * Coord2.z);
  double Yresult  = 
    z * (Coord1.y * Coord2.z - Coord1.z * Coord2.y) -
      x * (Coord1.x * Coord2.y - Coord1.y * Coord2.x);
  z = 
    x * (Coord1.z * Coord2.x - Coord1.x * Coord2.z) -
      y * (Coord1.y * Coord2.z - Coord1.z * Coord2.y);
  x = Xresult;
  y = Yresult;
}

 gp_XYZ gp_XYZ::CrossCrossed (const gp_XYZ& Coord1,
				    const gp_XYZ& Coord2) const
{
  gp_XYZ Coord0 = *this;
  Coord0.CrossCross (Coord1, Coord2);
  return Coord0;
}

 void gp_XYZ::Divide (const double Scalar)
{
  x /= Scalar;
  y /= Scalar;
  z /= Scalar;
}

 gp_XYZ gp_XYZ::Divided (const double Scalar) const {
  return gp_XYZ(x / Scalar,y / Scalar,z / Scalar);
}

 double gp_XYZ::Dot (const gp_XYZ& Other) const {
  return(x * Other.x + y * Other.y + z * Other.z);
}

 double gp_XYZ::DotCross (const gp_XYZ& Coord1,
				       const gp_XYZ& Coord2) const
{
  double Xresult = Coord1.y * Coord2.z - Coord1.z * Coord2.y;
  double Yresult = Coord1.z * Coord2.x - Coord1.x * Coord2.z;
  double Zresult = Coord1.x * Coord2.y - Coord1.y * Coord2.x;
  return ( x * Xresult + y * Yresult + z * Zresult);
}

 void gp_XYZ::Multiply (const double Scalar)
{
  x *= Scalar;
  y *= Scalar;
  z *= Scalar;
}

 void gp_XYZ::Multiply (const gp_XYZ& Other)
{
  x *= Other.x;
  y *= Other.y;
  z *= Other.z;
}

 void gp_XYZ::Multiply (const gp_Mat& Matrix)
{
  double Xresult = Matrix.matrix[0][0] * x + Matrix.matrix[0][1] * y + Matrix.matrix[0][2] * z;
  double Yresult = Matrix.matrix[1][0] * x + Matrix.matrix[1][1] * y + Matrix.matrix[1][2] * z;
  z                     = Matrix.matrix[2][0] * x + Matrix.matrix[2][1] * y + Matrix.matrix[2][2] * z;
  x                     = Xresult;
  y                     = Yresult;
}

 gp_XYZ gp_XYZ::Multiplied (const double Scalar) const {
  return gp_XYZ(x * Scalar,y * Scalar,z * Scalar);
}

 gp_XYZ gp_XYZ::Multiplied (const gp_XYZ& Other) const {
  return gp_XYZ(x * Other.x, y * Other.y, z * Other.z);
}

 gp_XYZ gp_XYZ::Multiplied (const gp_Mat& Matrix) const
{
  return gp_XYZ (Matrix.matrix[0][0] * x + Matrix.matrix[0][1] * y + Matrix.matrix[0][2] * z,
		 Matrix.matrix[1][0] * x + Matrix.matrix[1][1] * y + Matrix.matrix[1][2] * z,
		 Matrix.matrix[2][0] * x + Matrix.matrix[2][1] * y + Matrix.matrix[2][2] * z);
}

 void gp_XYZ::Normalize ()
{
  double D = Modulus();
  Standard_ConstructionError_Raise_if(D <= gp::Resolution(), "");
  x = x / D;  y = y / D;  z = z / D;
}

 gp_XYZ gp_XYZ::Normalized () const
{
  double D = Modulus();
  Standard_ConstructionError_Raise_if(D <= gp::Resolution(), "");
  return gp_XYZ (x / D, y / D, z / D);
}

 void gp_XYZ::Reverse ()
{
  x = - x;
  y = - y;
  z = - z;
}

 gp_XYZ gp_XYZ::Reversed () const
{
  return gp_XYZ(-x, -y,	-z);
}

 void gp_XYZ::Subtract (const gp_XYZ& Right)
{
  x-=Right.x;
  y-=Right.y;
  z-=Right.z;
}

 gp_XYZ gp_XYZ::Subtracted (const gp_XYZ& Right) const
{
  return gp_XYZ(x - Right.x, y - Right.y, z - Right.z);
}

 void gp_XYZ::SetLinearForm (const double L, 
				   const gp_XYZ& Left,
				   const double R, 
				   const gp_XYZ& Right) {
  
  x = L * Left.x + R * Right.x;
  y = L * Left.y + R * Right.y;
  z = L * Left.z + R * Right.z; 
}

 void gp_XYZ::SetLinearForm(const double L, 
				  const gp_XYZ& Left,
				  const gp_XYZ& Right) {
  x = L * Left.x + Right.x;
  y = L * Left.y + Right.y;
  z = L * Left.z + Right.z;
}

 void gp_XYZ::SetLinearForm (const gp_XYZ& Left, const gp_XYZ& Right) {
  x = Left.x + Right.x;
  y = Left.y + Right.y;
  z = Left.z + Right.z;
}

 void gp_XYZ::SetLinearForm (const double A1, const gp_XYZ& XYZ1,
				   const double A2, const gp_XYZ& XYZ2, 
				   const double A3, const gp_XYZ& XYZ3) {
  
  x = A1 * XYZ1.x + A2 * XYZ2.x + A3 * XYZ3.x;
  y = A1 * XYZ1.y + A2 * XYZ2.y + A3 * XYZ3.y;
  z = A1 * XYZ1.z + A2 * XYZ2.z + A3 * XYZ3.z;  
}

 void gp_XYZ::SetLinearForm (const double A1, const gp_XYZ& XYZ1,
				   const double A2, const gp_XYZ& XYZ2, 
				   const gp_XYZ& XYZ3) {
  x = A1 * XYZ1.x + A2 * XYZ2.x + XYZ3.x;
  y = A1 * XYZ1.y + A2 * XYZ2.y + XYZ3.y;
  z = A1 * XYZ1.z + A2 * XYZ2.z + XYZ3.z;
}

 void gp_XYZ::SetLinearForm (const double A1, const gp_XYZ& XYZ1,
				   const double A2, const gp_XYZ& XYZ2, 
				   const double A3, const gp_XYZ& XYZ3,
				   const gp_XYZ& XYZ4) {
  x = A1 * XYZ1.x + A2 * XYZ2.x + A3 * XYZ3.x + XYZ4.x;
  y = A1 * XYZ1.y + A2 * XYZ2.y + A3 * XYZ3.y + XYZ4.y;
  z = A1 * XYZ1.z + A2 * XYZ2.z + A3 * XYZ3.z + XYZ4.z;
  
}

 gp_XYZ operator* (const gp_Mat& Matrix, const gp_XYZ& Coord1) {
  return Coord1.Multiplied (Matrix);
}

 gp_XYZ operator* (const double Scalar, const gp_XYZ& Coord1) {
  return Coord1.Multiplied (Scalar);
}

 bool gp_XYZ::IsEqual (const gp_XYZ& Other,
	 const double Tolerance) const {
		 double val;
		 val = x - Other.x;
		 if (val < 0) val = - val;
		 if (val > Tolerance) return false;
		 val = y - Other.y;
		 if (val < 0) val = - val;
		 if (val > Tolerance) return false;
		 val = z - Other.z;
		 if (val < 0) val = - val;
		 if (val > Tolerance) return false;
		 return true;
 }




